//-----------------------------------------------------------------------
//  This file is part of Microsoft Robotics Developer Studio Code Samples.
// 
//  Copyright (C) Microsoft Corporation.  All rights reserved.
//
//  $File: VisionForm.cs $ $Revision: 1 $
//----------------------------------------------------------------------

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Drawing.Drawing2D;
using System.Text;
using System.Windows.Forms;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.Services.ControlPanel;

namespace HCC.Robotics.HCCSimpleVisionModule
{
    partial class VisionForm : Form
    {
        HCCSimpleVisionModuleOperations _mainPort;
        ImageProcessingResult[] _result;

        private Bitmap _cameraImage;
        string[] data = new string[17];

        public Bitmap CameraImage
        {
            get { return _cameraImage; }
            set
            {
                _cameraImage = value;

                Image old = picCamera.Image;
                picCamera.Image = value;

                if (old != null)
                {
                    old.Dispose();
                }
            }
        }

        private Bitmap _testImage;

        public Bitmap TestImage
        {
            get { return _testImage; }
            set
            {
                _testImage = value;

                Image old = picTest.Image;
                picTest.Image = value;

                if (old != null)
                {
                    old.Dispose();
                }
            }
        }

        private Bitmap _testImage2;

        public Bitmap TestImage2
        {
            get { return _testImage2; }
            set
            {
                _testImage2 = value;

                Image old = picTest2.Image;
                picTest2.Image = value;

                if (old != null)
                {
                    old.Dispose();
                }
            }
        }

        private Bitmap _faceImage;

        public Bitmap FaceImage
        {
            get { return _faceImage; }
            set
            {
                _faceImage = value;

                Image old = picFace.Image;
                picFace.Image = value;

                if (old != null)
                {
                    old.Dispose();
                }
            }
        }

        public VisionForm(HCCSimpleVisionModuleOperations mainPort, ImageProcessingResult/*IT3*/[] result)
        {
            InitializeComponent();

            _mainPort = mainPort;
            _result = result;

        }

        private void VisionForm_KeyDown(object sender, KeyEventArgs e)
        {
        }

        private void VisionForm_KeyPress(object sender, KeyPressEventArgs e)
        {
        }

        private void VisionForm_KeyUp(object sender, KeyEventArgs e)
        {
        }

        bool _capturing;
        Point _center;
        int _radius;
        bool _ready = false;

        private void picCamera_MouseDown(object sender, MouseEventArgs e)
        {
            if (radioButton_CalibrateColors.Checked)
                startCapturing(sender, e);
        }

        private void startCapturing(object sender, MouseEventArgs e)
        {
            _capturing = true;
            _center = new Point(e.X, e.Y);
        }

        private void picCamera_MouseMove(object sender, MouseEventArgs e)
        {
            if (_capturing)
            {
                int dx = e.X - _center.X;
                int dy = e.Y - _center.Y;

                _radius = (int)Math.Round(
                    Math.Sqrt(dx * dx + dy * dy)
                );

                picCamera.Invalidate();
            }
        }


        private void picCamera_MouseUp(object sender, MouseEventArgs e)
        {
            if(radioButton_CalibrateColors.Checked ) 
                stopCapturing(sender, e);
            else
                registerField(sender, e);
        }

        private void stopCapturing(object sender, MouseEventArgs e)
        {
            if (_capturing)
            {
                int dx = e.X - _center.X;
                int dy = e.Y - _center.Y;

                _radius = (int)Math.Round(
                    Math.Sqrt(dx * dx + dy * dy)
                );

                _capturing = false;
                _ready = true;

                picCamera.Invalidate();
            }
        }
        private void picCamera_Paint(object sender, PaintEventArgs e)
        {
            double width, height;

            if (_capturing || _ready)
            {
                e.Graphics.DrawEllipse(
                    Pens.Black,
                    _center.X - _radius,
                    _center.Y - _radius,
                    _radius * 2,
                    _radius * 2
                );
                e.Graphics.DrawEllipse(
                    Pens.White,
                    _center.X - _radius - 1,
                    _center.Y - _radius - 1,
                    _radius * 2 + 2,
                    _radius * 2 + 2
                );
            }

            //IT2 if (_result.ObjectFound)
            //{
            //    width = 3 * _result.XStdDev;
            //    height = 3 * _result.YStdDev;

            //    Rectangle rect = new Rectangle(
            //        (int)(_result.XMean - width / 2),
            //        (int)(_result.YMean - height / 2),
            //        (int)width,
            //        (int)height
            //    );

            //    e.Graphics.DrawRectangle(Pens.White, rect);
            //    rect.Inflate(1, 1);
            //    e.Graphics.DrawRectangle(Pens.Black, rect);
            //}

            // Draw center of motion

            if (_result[0]!=null && _result/*IT3*/[0].MotionFound)
            {
                width = height = 15;
                Rectangle rect = new Rectangle(
                    (int)(_result/*IT3*/[0].XMotion - width / 2),
                    (int)(_result/*IT3*/[0].YMotion - height / 2),
                    (int)width,
                    (int)height
                );
                e.Graphics.DrawRectangle(Pens.Red, rect);
            }

            //ITif (_result.LeftHandX > 0)
            //{
            //    width = height = 15;
            //    Rectangle rect = new Rectangle(
            //        (int)(_result.LeftHandX - width / 2),
            //        (int)(_result.LeftHandY - height / 2),
            //        (int)width,
            //        (int)height
            //    );
            //    e.Graphics.DrawRectangle(Pens.Orange, rect);

            //}
            //IT if (_result.RightHandX > 0)
            //{
            //    width = height = 15;
            //    Rectangle rect = new Rectangle(
            //        (int)(_result.RightHandX - width / 2),
            //        (int)(_result.RightHandY - height / 2),
            //        (int)width,
            //        (int)height
            //    );
            //    e.Graphics.DrawRectangle(Pens.Orange, rect);

            //}

            //IT if (_result.HeadFound)
            //{
            //    Rectangle rect = new Rectangle(
            //        _result.HeadBoxRegion.Sx,
            //        _result.HeadBoxRegion.Sy,
            //        _result.HeadBoxRegion.Ex - _result.HeadBoxRegion.Sx,
            //        _result.HeadBoxRegion.Ey - _result.HeadBoxRegion.Sy
            //    );
            //    e.Graphics.DrawRectangle(Pens.Yellow, rect);
            //    rect.Inflate(1, 1);
            //    e.Graphics.DrawRectangle(Pens.Yellow, rect);
            //}

            //IT2 if (_result.HeadFound || _result.ObjectFound)
            //{
            //    if (_result.LeftHandGestureFound && _result.RightHandGestureFound)
            //        handGesture.Text = "BOTH";
            //    else if (_result.LeftHandGestureFound)
            //        handGesture.Text = "LEFT";
            //    else if (_result.RightHandGestureFound)
            //        handGesture.Text = "RIGHT";
            //    else
            //        handGesture.Text = "NONE";
            //}
            //else
            //    handGesture.Text = "NONE";
            //IT3 hier the Segmentations are drawn
            /*IT3*/
            for (int colorIndex = 0; colorIndex < _result.Length; colorIndex++)
            {
                if (_result[colorIndex] != null)
                {
                    if (_result/*IT3*/[colorIndex].numberOfLocations > 0)
                    {
                        Pen ThePen;
                        switch (_result/*IT3*/[colorIndex].colorVector.Name)
                        {
                            case ColorVector.TrackingColor.Ball:
                                ThePen = Pens.OrangeRed;
                                break;
                            case ColorVector.TrackingColor.Yellow:
                                ThePen = Pens.Yellow;
                                break;
                            case ColorVector.TrackingColor.Blue:
                                ThePen = Pens.Blue;
                                break;
                            case ColorVector.TrackingColor.Pink:
                                ThePen = Pens.Pink;
                                break;
                            default:
                                ThePen = Pens.White;
                                break;
                        }
                        for (int i = 0; i < _result/*IT3*/[colorIndex].numberOfLocations; i++)
                        {
                            Rectangle rect = new Rectangle(
                                _result/*IT3*/[colorIndex].boundBox[i].Sx,
                                _result/*IT3*/[colorIndex].boundBox[i].Sy,
                                _result/*IT3*/[colorIndex].boundBox[i].Ex - _result/*IT3*/[colorIndex].boundBox[i].Sx,
                                _result/*IT3*/[colorIndex].boundBox[i].Ey - _result/*IT3*/[colorIndex].boundBox[i].Sy
                            );
                            e.Graphics.DrawRectangle(ThePen, rect);
                            //e.Graphics.FillEllipse(ThePen, rect);
                        }
                    }
                    if(_result[colorIndex].numberOfTrackedBots>0)
                    {
                        Pen directionPen = Pens.Black;
                        //e.Graphics.DrawLine(directionPen, new Point(0, 0), new Point(640, 480));
                        //directionPen.Width =10;
                        for (int i = 0; i < _result[colorIndex].numberOfTrackedBots; i++)
                        {
                            int botX = _result[colorIndex].botTracks[i].TrackX;
                            int botY = _result[colorIndex].botTracks[i].TrackY;
                            int trackDirection = _result[colorIndex].botTracks[i].TrackDirection;
                            double trackDirectionRadians = (((double)trackDirection)* Math.PI) /180;
                            double cosAngle = Math.Cos(trackDirectionRadians);
                            double sinAngle = Math.Sin(trackDirectionRadians);
                            int botXDir = botX + (int) (cosAngle * 100);
                            int botYDir = botY - (int) (sinAngle * 100);
                            e.Graphics.DrawLine(directionPen, new Point(botX, botY), new Point(botXDir, botYDir));
                        }
                    }
                }
            }
        }
        private double[,] ColorCalibrateValues = new double[9, 4];
        private void CalibrateColorButton_Click(object sender, EventArgs e)
        {
            if (_radius == 0 || !_ready)
            {
                return;
            }

            Bitmap bmp = picCamera.Image as Bitmap;
            if (bmp == null)
            {
                return;
            }

            int accumRed = 0;
            int accumGreen = 0;
            int accumBlue = 0;
            int count = 0;

            int[] redProjection = new int[256];
            int[] greenProjection = new int[256];
            int[] blueProjection = new int[256];

            for (int y = _center.Y - _radius; y <= _center.Y + _radius; y++)
            {
                if (y < 0 || y >= bmp.Height)
                {
                    continue;
                }

                for (int x = _center.X - _radius; x <= _center.X + _radius; x++)
                {
                    if (x < 0 || x >= bmp.Width)
                    {
                        continue;
                    }

                    int dx = _center.X - x;
                    int dy = _center.Y - y;

                    if (dx * dx + dy * dy > _radius)
                    {
                        continue;
                    }

                    Color color = bmp.GetPixel(x, y);

                    count++;
                    accumRed += color.R;
                    accumGreen += color.G;
                    accumBlue += color.B;

                    redProjection[color.R]++;
                    greenProjection[color.G]++;
                    blueProjection[color.B]++;
                }
            }

            double meanRed = (double)accumRed / count;
            double meanGreen = (double)accumGreen / count;
            double meanBlue = (double)accumBlue / count;

            double redDev = 1 + CalculateDeviation(meanRed, redProjection);
            double greenDev = 1 + CalculateDeviation(meanGreen, greenProjection);
            double blueDev = 1 + CalculateDeviation(meanBlue, blueProjection);

            int RedMin = (int)Math.Round(meanRed - redDev);
            int RedMax = (int)Math.Round(meanRed + redDev);
            int GreenMin = (int)Math.Round(meanGreen - greenDev);
            int GreenMax = (int)Math.Round(meanGreen + greenDev);
            int BlueMin = (int)Math.Round(meanBlue - blueDev);
            int BlueMax = (int)Math.Round(meanBlue + blueDev);

            int ColorCalibrateRow = 0;
            if (BallRadioButton.Checked)
            {
                ColorCalibrateRow = 0;
            }
            else if (YellowRadioButton.Checked)
            {
                ColorCalibrateRow = 1;
            }
            else if (BlueRadioButton.Checked)
            {
                ColorCalibrateRow = 2;
            }
            else if (PinkRadioButton.Checked)
            {
                ColorCalibrateRow = 3;
            }

            double sum = meanRed + meanGreen + meanBlue;
            if (sum == 0) return;
            double nR = (double)meanRed / (double)sum;
            double nG = (double)meanGreen / (double)sum;
            double nB = (double)meanBlue / (double)sum;

            ColorCalibrateValues[0, ColorCalibrateRow] = RedMin;
            ColorCalibrateValues[1, ColorCalibrateRow] = RedMax;
            ColorCalibrateValues[2, ColorCalibrateRow] = nR;
            ColorCalibrateValues[3, ColorCalibrateRow] = GreenMin;
            ColorCalibrateValues[4, ColorCalibrateRow] = GreenMax; ;
            ColorCalibrateValues[5, ColorCalibrateRow] = nG;
            ColorCalibrateValues[6, ColorCalibrateRow] = BlueMin;
            ColorCalibrateValues[7, ColorCalibrateRow] = BlueMax;
            ColorCalibrateValues[8, ColorCalibrateRow] = nB;
            int CountControls = 0;

            int ControlColumn, controlRow;
            foreach (Control c in this.tableLayoutPanel1.Controls)
            {
                ControlColumn = tableLayoutPanel1.GetCellPosition(c).Column;
                controlRow = tableLayoutPanel1.GetCellPosition(c).Row;
                if (ControlColumn == 0 | controlRow == 0)
                {
                }
                else if (controlRow == ColorCalibrateRow + 1)
                {
                    c.Text = ColorCalibrateValues[ControlColumn - 1, ColorCalibrateRow].ToString();
                }
                CountControls++;
            }


            double SimilarityMeasure = Convert.ToDouble(textSmilarity.Text);

            if (SimilarityMeasure == 0) SimilarityMeasure = 0.993;

            _mainPort.Post(new SetObjectTrackingColor(new ColorVector(ColorVector.TrackingColor.Ball,
                    ColorCalibrateValues[2, 0],
                    ColorCalibrateValues[5, 0],
                    ColorCalibrateValues[8, 0],
                    SimilarityMeasure)));
            _mainPort.Post(new SetObjectTrackingColor(new ColorVector(ColorVector.TrackingColor.Yellow,
                    ColorCalibrateValues[2, 1],
                    ColorCalibrateValues[5, 1],
                    ColorCalibrateValues[8, 1],
                    SimilarityMeasure)));
            _mainPort.Post(new SetObjectTrackingColor(new ColorVector(ColorVector.TrackingColor.Blue,
                    ColorCalibrateValues[2, 2],
                    ColorCalibrateValues[5, 2],
                    ColorCalibrateValues[8, 2],
                    SimilarityMeasure)));
            _mainPort.Post(new SetObjectTrackingColor(new ColorVector(ColorVector.TrackingColor.Pink,
                    ColorCalibrateValues[2, 3],
                    ColorCalibrateValues[5, 3],
                    ColorCalibrateValues[8, 3],
                    SimilarityMeasure)));
        }

        private static double CalculateDeviation(double mean, int[] projection)
        {
            int count = 0;
            double variance = 0;

            for (int i = 0; i < projection.Length; i++)
            {
                if (projection[i] > 0)
                {
                    double offset = i - mean;

                    variance += offset * offset * projection[i];
                    count += projection[i];
                }
            }

            if (count == 0)
            {
                return 0;
            }

            return Math.Sqrt(variance / count);
        }

        protected override void OnClosed(EventArgs e)
        {
            base.OnClosed(e);

            _mainPort.Post(new DsspDefaultDrop());
        }

        private void VisionForm_FormClosing(object sender, FormClosingEventArgs e)
        {
            _mainPort.Post(new DsspDefaultDrop());
        }

        private void registerField(object sender, MouseEventArgs e)
        {
            if (btnTL.Checked)
            {
                data[0] = e.X.ToString();
                data[8] = e.Y.ToString();
                btnTL.Text = "Top Left : " + data[0] + ", " + data[8];
            }
            if (btnTR.Checked)
            {
                data[1] = e.X.ToString();
                data[9] = e.Y.ToString();
                btnTR.Text = "Top Right : " + data[1] + ", " + data[9];
            }
            if (btnBL.Checked)
            {
                data[2] = e.X.ToString();
                data[10] = e.Y.ToString();
                btnBL.Text = "Bottom Left : " + data[2] + ", " + data[10];
            }
            if (btnBR.Checked)
            {
                data[3] = e.X.ToString();
                data[11] = e.Y.ToString();
                btnBR.Text = "Bottom Right : " + data[3] + ", " + data[11];
            }
            if (btnLGU.Checked)
            {
                data[4] = e.X.ToString();
                data[12] = e.Y.ToString();
                btnLGU.Text = "Left Goal Upper : " + data[4] + ", " + data[12];
            }
            if (btnLGL.Checked)
            {
                data[5] = e.X.ToString();
                data[13] = e.Y.ToString();
                btnLGL.Text = "Left Goal Lower : " + data[5] + ", " + data[13];
            }
            if (btnRGU.Checked)
            {
                data[6] = e.X.ToString();
                data[14] = e.Y.ToString();
                btnRGU.Text = "Right Goal Upper : " + data[6] + ", " + data[14];
            }
            if (btnRGL.Checked)
            {
                data[7] = e.X.ToString();
                data[15] = e.Y.ToString();
                btnRGL.Text = "Right Goal Lower : " + data[7] + ", " + data[15];
            }
        }
        FieldMeasures fieldMeasures = new FieldMeasures();
        private void buttonPostFieldData_Click(object sender, EventArgs e)
        {
            int tmp;
            int.TryParse(data[0], out tmp);
            fieldMeasures.TopLeftX = tmp;
            int.TryParse(data[1], out tmp);
            fieldMeasures.TopRightX = tmp;
            int.TryParse(data[2], out tmp);
            fieldMeasures.BottomLeftX = tmp;
            int.TryParse(data[3], out tmp);
            fieldMeasures.BottomRightX = tmp;

            int.TryParse(data[4], out tmp);
            fieldMeasures.LeftGoalUpperX = tmp;
            int.TryParse(data[5], out tmp);
            fieldMeasures.LeftGoalLowerX = tmp;

            int.TryParse(data[6], out tmp);
            fieldMeasures.RightGoalUpperX = tmp;
            int.TryParse(data[7], out tmp);
            fieldMeasures.RightGoalLowerX = tmp;

            int.TryParse(data[8], out tmp);
            fieldMeasures.TopLeftY = tmp;
            int.TryParse(data[9], out tmp);
            fieldMeasures.TopRightY = tmp;
            int.TryParse(data[10], out tmp);
            fieldMeasures.BottomLeftY = tmp;
            int.TryParse(data[11], out tmp);
            fieldMeasures.BottomRightY = tmp;

            int.TryParse(data[12], out tmp);
            fieldMeasures.LeftGoalUpperY = tmp;
            int.TryParse(data[13], out tmp);
            fieldMeasures.LeftGoalLowerY = tmp;

            int.TryParse(data[14], out tmp);
            fieldMeasures.RightGoalUpperY = tmp;
            int.TryParse(data[15], out tmp);
            fieldMeasures.RightGoalLowerY = tmp;

            _mainPort.Post(new UpdateFieldMeasures(fieldMeasures));
        }
    }
}